#include "ti_msp_dl_config.h"
#include "Variables.h"
#define FRAME_HEADER_1 0x55
#define FRAME_HEADER_2 0x53
#define FRAME_LENGTH   11

volatile uint8_t rx_buffer[FRAME_LENGTH];
volatile uint8_t rx_index = 0;
volatile uint8_t parse_state = 0;
volatile uint8_t checksum = 0;

volatile float yaw_angle = 0.0f;  // Z轴角度（偏航角）

void UART_JY901S_INST_IRQHandler(void)
{
    uint8_t rx = DL_UART_Main_receiveData(UART_JY901S_INST);

    switch (parse_state)
    {
        case 0: // 等待帧头 0x55
            if (rx == FRAME_HEADER_1) {
                rx_index = 0;
                checksum = rx;
                parse_state = 1;
            }
            break;

        case 1: // 等待帧头 0x53
            if (rx == FRAME_HEADER_2) {
                checksum += rx;
                parse_state = 2;
            } else {
                parse_state = 0;
            }
            break;

        case 2: // 接收后续 9 字节数据
            rx_buffer[rx_index++] = rx;
            checksum += rx;

            if (rx_index >= 8) {
                parse_state = 3;
            }
            break;

        case 3: // 校验和校验
            if (checksum == rx)
            {
                // 正确接收，提取 Yaw（偏航角）
                int16_t yaw_raw = (rx_buffer[5] << 8) | rx_buffer[4]; // YawH << 8 | YawL
                yaw_angle = ((float)yaw_raw / 32768.0f) * 180.0f;
                // 可添加其他处理逻辑
            }
            // 无论校验是否通过都重置
            parse_state = 0;
            break;

        default:
            parse_state = 0;
            break;
    }
}

void JY901S_RESET_YAW()
{
    DL_UART_Main_transmitDataBlocking(UART_JY901S_INST,0XFF);
	DL_UART_Main_transmitDataBlocking(UART_JY901S_INST,0XAA);
	DL_UART_Main_transmitDataBlocking(UART_JY901S_INST,0X69);
	DL_UART_Main_transmitDataBlocking(UART_JY901S_INST,0X88);
	DL_UART_Main_transmitDataBlocking(UART_JY901S_INST,0XB5);
	delay_ms1(100);
	DL_UART_Main_transmitDataBlocking(UART_JY901S_INST,0XFF);
	DL_UART_Main_transmitDataBlocking(UART_JY901S_INST,0XAA);
	DL_UART_Main_transmitDataBlocking(UART_JY901S_INST,0X01);
	DL_UART_Main_transmitDataBlocking(UART_JY901S_INST,0X04);
	DL_UART_Main_transmitDataBlocking(UART_JY901S_INST,0X00);
	delay_ms1(100);
	DL_UART_Main_transmitDataBlocking(UART_JY901S_INST,0XFF);
	DL_UART_Main_transmitDataBlocking(UART_JY901S_INST,0XAA);
	DL_UART_Main_transmitDataBlocking(UART_JY901S_INST,0X00);
	DL_UART_Main_transmitDataBlocking(UART_JY901S_INST,0X00);
	DL_UART_Main_transmitDataBlocking(UART_JY901S_INST,0X00);

}